﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;
using starLiGHT.Collections;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: n/a
// Revision   : n/a
// Change-Date: n/a
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2CollidePolygon.cpp
// Revision   : r111
// Change-Date: 2010-06-13
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public partial class Collision
    { 
        /// <summary>
        /// Find the separation between poly1 and poly2 for a give edge normal on poly1.
        /// </summary>
        /// <param name="poly1"></param>
        /// <param name="xf1"></param>
        /// <param name="edge1"></param>
        /// <param name="poly2"></param>
        /// <param name="xf2"></param>
        /// <returns></returns>
        internal static float EdgeSeparation(ref PolygonShape poly1, ref Transform xf1, ref int edge1,
							                 ref PolygonShape poly2, ref Transform xf2)
        {
	        int count1 = poly1.m_vertexCount;
	        Vector2[] vertices1 = poly1.m_vertices;
	        Vector2[] normals1 = poly1.m_normals;

	        int count2 = poly2.m_vertexCount;
	        Vector2[] vertices2 = poly2.m_vertices;

	        System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < count1);

	        // Convert normal from poly1's frame into poly2's frame.
#if !OPTIMIZE
            Vector2 normal1World = Math.Mul(xf1.R, normals1[edge1]);
	        Vector2 normal1 = Math.MulT(xf2.R, normal1World);
#else
            Vector2 n1 = normals1[edge1];
            Vector2 normal1World;
            normal1World.X = xf1.R.col1.X * n1.X + xf1.R.col2.X * n1.Y;
            normal1World.Y = xf1.R.col1.Y * n1.X + xf1.R.col2.Y * n1.Y;

            Vector2 normal1;
            normal1.X = normal1World.X * xf2.R.col1.X + normal1World.Y * xf2.R.col1.Y;
            normal1.Y = normal1World.X * xf2.R.col2.X + normal1World.Y * xf2.R.col2.Y;
#endif

	        // Find support vertex on poly2 for -normal.
	        int index = 0;
	        float minDot = float.MaxValue;

	        for (int i = 0; i < count2; ++i)
	        {
#if !OPTIMIZE
                float dot = Vector2.Dot(vertices2[i], normal1);
#else
                float dot; dot = vertices2[i].X * normal1.X + vertices2[i].Y * normal1.Y; // Vector2.Dot(ref vertices2[i], ref normal1, out dot);
#endif
		        if (dot < minDot)
		        {
			        minDot = dot;
			        index = i;
		        }
	        }

#if !OPTIMIZE
            Vector2 v1 = Math.Mul(xf1, vertices1[edge1]);
	        Vector2 v2 = Math.Mul(xf2, vertices2[index]);
	        float separation = Vector2.Dot(v2 - v1, normal1World);
#else
            Vector2 e1 = vertices1[edge1];
            Vector2 e2 = vertices2[index];

            Vector2 v1;
            v1.X = xf1.position.X + xf1.R.col1.X * e1.X + xf1.R.col2.X * e1.Y;
            v1.Y = xf1.position.Y + xf1.R.col1.Y * e1.X + xf1.R.col2.Y * e1.Y;

            Vector2 v2;
            v2.X = xf2.position.X + xf2.R.col1.X * e2.X + xf2.R.col2.X * e2.Y;
            v2.Y = xf2.position.Y + xf2.R.col1.Y * e2.X + xf2.R.col2.Y * e2.Y;

            Vector2 v = new Vector2(); v.X = v2.X - v1.X; v.Y = v2.Y - v1.Y;
            float separation = v.X * normal1World.X + v.Y * normal1World.Y; // Vector2.Dot(ref v, ref normal1World, out separation);
#endif
	        return separation;
        }

        /// <summary>
        /// Find the max separation between poly1 and poly2 using edge normals from poly1.
        /// </summary>
        /// <param name="edgeIndex"></param>
        /// <param name="poly1"></param>
        /// <param name="xf1"></param>
        /// <param name="poly2"></param>
        /// <param name="xf2"></param>
        /// <returns></returns>
        internal static float FindMaxSeparation(out int edgeIndex,
								                ref PolygonShape poly1, ref Transform xf1,
								                ref PolygonShape poly2, ref Transform xf2)
        {
	        int count1 = poly1.m_vertexCount;
	        Vector2[] normals1 = poly1.m_normals;

	        // Vector pointing from the centroid of poly1 to the centroid of poly2.
#if !OPTIMIZE
            Vector2 d = Math.Mul(xf2, poly2.m_centroid) - Math.Mul(xf1, poly1.m_centroid);
	        Vector2 dLocal1 = Math.MulT(xf1.R, d);
#else
            Vector2 d;
            d.X = xf2.position.X + xf2.R.col1.X * poly2.m_centroid.X + xf2.R.col2.X * poly2.m_centroid.Y - xf1.position.X + xf1.R.col1.X * poly1.m_centroid.X + xf1.R.col2.X * poly1.m_centroid.Y;
            d.Y = xf2.position.Y + xf2.R.col1.Y * poly2.m_centroid.X + xf2.R.col2.Y * poly2.m_centroid.Y - xf1.position.Y + xf1.R.col1.Y * poly1.m_centroid.X + xf1.R.col2.Y * poly1.m_centroid.Y;
            
            Vector2 dLocal1;
            dLocal1.X = d.X * xf1.R.col1.X + d.Y * xf1.R.col1.Y;
            dLocal1.Y = d.X * xf1.R.col2.X + d.Y * xf1.R.col2.Y;
#endif

	        // Find edge normal on poly1 that has the largest projection onto d.
	        int edge = 0;
	        float maxDot = -float.MaxValue;
	        for (int i = 0; i < count1; ++i)
	        {
#if !OPTIMIZE
                float dot = Vector2.Dot(normals1[i], dLocal1);
#else
                float dot = normals1[i].X * dLocal1.X + normals1[i].Y * dLocal1.Y; // Vector2.Dot(ref normals1[i], ref dLocal1, out dot);
#endif
		        if (dot > maxDot)
		        {
			        maxDot = dot;
			        edge = i;
		        }
	        }

	        // Get the separation for the edge normal.
            float s = EdgeSeparation(ref poly1, ref xf1, ref edge, ref poly2, ref xf2);

	        // Check the separation for the previous edge normal.
	        int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
            float sPrev = EdgeSeparation(ref poly1, ref xf1, ref prevEdge, ref poly2, ref xf2);

	        // Check the separation for the next edge normal.
	        int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
            float sNext = EdgeSeparation(ref poly1, ref xf1, ref nextEdge, ref poly2, ref xf2);

	        // Find the best edge and the search direction.
	        int bestEdge;
	        float bestSeparation;
	        int increment;
	        if (sPrev > s && sPrev > sNext)
	        {
		        increment = -1;
		        bestEdge = prevEdge;
		        bestSeparation = sPrev;
	        }
	        else if (sNext > s)
	        {
		        increment = 1;
		        bestEdge = nextEdge;
		        bestSeparation = sNext;
	        }
	        else
	        {
		        edgeIndex = edge;
		        return s;
	        }

	        // Perform a local search for the best edge normal.
	        for (; ;)
	        {
		        if (increment == -1)
			        edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
		        else
			        edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;

                s = EdgeSeparation(ref poly1, ref xf1, ref edge, ref poly2, ref xf2);

		        if (s > bestSeparation)
		        {
			        bestEdge = edge;
			        bestSeparation = s;
		        }
		        else
		        {
			        break;
		        }
	        }

	        edgeIndex = bestEdge;
	        return bestSeparation;
        }

        static ClipVertex[] clipVertexCache = new ClipVertex[2];

        internal static void FindIncidentEdge(out ClipVertex[] c,
							                  ref PolygonShape poly1, ref Transform xf1, int edge1,
							                  ref PolygonShape poly2, ref Transform xf2)
        {
	        int count1 = poly1.m_vertexCount;
	        Vector2[] normals1 = poly1.m_normals;

	        int count2 = poly2.m_vertexCount;
	        Vector2[] vertices2 = poly2.m_vertices;
	        Vector2[] normals2 = poly2.m_normals;

	        System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < count1);

	        // Get the normal of the reference edge in poly2's frame.
#if !OPTIMIZE
            Vector2 normal1 = Math.MulT(xf2.R, Math.Mul(xf1.R, normals1[edge1]));
#else
            Vector2 normal1;
            Vector2 e1 = normals1[edge1];
            
            Vector2 t;
            t.X = xf1.R.col1.X * e1.X + xf1.R.col2.X * e1.Y;
            t.Y = xf1.R.col1.Y * e1.X + xf1.R.col2.Y * e1.Y;

            normal1.X = t.X * xf2.R.col1.X + t.Y * xf2.R.col1.Y; // Vector2.Dot(ref t, ref xf2.R.col1, out normal1.X);
            normal1.Y = t.X * xf2.R.col2.X + t.Y * xf2.R.col2.Y; // Vector2.Dot(ref t, ref xf2.R.col2, out normal1.Y);
#endif

	        // Find the incident edge on poly2.
	        int index = 0;
	        float minDot = float.MaxValue;
	        for (int i = 0; i < count2; ++i)
	        {
#if !OPTIMIZE
                float dot = Vector2.Dot(normal1, normals2[i]);
#else
                float dot = normal1.X * normals2[i].X + normal1.Y * normals2[i].Y; // Vector2.Dot(ref normal1, ref normals2[i], out dot);
#endif
		        if (dot < minDot)
		        {
			        minDot = dot;
			        index = i;
		        }
	        }

	        // Build the clip vertices for the incident edge.
	        int i1 = index;
	        int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

            c = new ClipVertex[2];

#if !OPTIMIZE
            c[0].v = Math.Mul(xf2, vertices2[i1]);
            c[0].id.cf.indexA = (byte)edge1;
            c[0].id.cf.indexB = (byte)i1;
            c[0].id.cf.typeA = ContactFeature.Type.FACE;
            c[0].id.cf.typeB = ContactFeature.Type.VERTEX;

	        c[1].v = Math.Mul(xf2, vertices2[i2]);
            c[1].id.cf.indexA = (byte)edge1;
            c[1].id.cf.indexB = (byte)i2;
            c[1].id.cf.typeA = ContactFeature.Type.FACE;
            c[1].id.cf.typeB = ContactFeature.Type.VERTEX;
#else
            Vector2 ti = vertices2[i1];
            c[0].v.X = xf2.position.X + xf2.R.col1.X * ti.X + xf2.R.col2.X * ti.Y;
            c[0].v.Y = xf2.position.Y + xf2.R.col1.Y * ti.X + xf2.R.col2.Y * ti.Y;
            c[0].id.cf.indexA = (byte)edge1;
            c[0].id.cf.indexB = (byte)i1;
            c[0].id.cf.typeA = ContactFeature.Type.FACE;
            c[0].id.cf.typeB = ContactFeature.Type.VERTEX;

            ti = vertices2[i2];
            c[1].v.X = xf2.position.X + xf2.R.col1.X * ti.X + xf2.R.col2.X * ti.Y;
            c[1].v.Y = xf2.position.Y + xf2.R.col1.Y * ti.X + xf2.R.col2.Y * ti.Y;
            c[1].id.cf.indexA = (byte)edge1;
            c[1].id.cf.indexB = (byte)i2;
            c[1].id.cf.typeA = ContactFeature.Type.FACE;
            c[1].id.cf.typeB = ContactFeature.Type.VERTEX;
#endif
        }

#if !ZUNE && !WINDOWS_PHONE && !SILVERLIGHT
        static ClipVertex[] clipPoints1 = new ClipVertex[2];
        static ClipVertex[] clipPoints2 = new ClipVertex[2];

        ///<summary>
        /// Find edge normal of max separation on A - return if separating axis is found<br/>
        /// Find edge normal of max separation on B - return if separation axis is found<br/>
        /// Choose reference edge as min(minA, minB)<br/>
        /// Find incident edge<br/>
        /// Clip<br/>
        /// <br/>
        /// The normal points from 1 to 2<br/>
        /// <param name="manifold"></param>
        /// <param name="polyA"></param>
        /// <param name="xfA"></param>
        /// <param name="polyB"></param>
        /// <param name="xfB"></param>
        /// </summary>
        public unsafe static void CollidePolygons(out Manifold manifold,
					                       PolygonShape polyA, ref Transform xfA,
					                       PolygonShape polyB, ref Transform xfB)
        {
#if !OPTIMIZE
            manifold = new Manifold();
#else
            manifold = ManifoldPool.Instance.Allocate();
#endif

	        manifold.pointCount = 0;
	        float totalRadius = polyA.m_radius + polyB.m_radius;

	        int edgeA = 0;
	        float separationA = FindMaxSeparation(out edgeA, ref polyA, ref xfA, ref polyB, ref xfB);
	        if (separationA > totalRadius)
		        return;

	        int edgeB = 0;
	        float separationB = FindMaxSeparation(out edgeB, ref polyB, ref xfB, ref polyA, ref xfA);
	        if (separationB > totalRadius)
		        return;

	        PolygonShape poly1;	// reference polygon
	        PolygonShape poly2;	// incident polygon
	        Transform xf1, xf2;
	        int edge1;		// reference edge
	        byte flip;
	        const float k_relativeTol = 0.98f;
	        const float k_absoluteTol = 0.001f;

	        if (separationB > k_relativeTol * separationA + k_absoluteTol)
	        {
		        poly1 = polyB;
		        poly2 = polyA;
		        xf1 = xfB;
		        xf2 = xfA;
		        edge1 = edgeB;
		        manifold.type = ManifoldType.e_faceB;
		        flip = 1;
	        }
	        else
	        {
		        poly1 = polyA;
		        poly2 = polyB;
		        xf1 = xfA;
		        xf2 = xfB;
		        edge1 = edgeA;
		        manifold.type = ManifoldType.e_faceA;
		        flip = 0;
	        }

	        ClipVertex[] incidentEdge;
	        FindIncidentEdge(out incidentEdge, ref poly1, ref xf1, edge1, ref poly2, ref xf2);

	        int count1 = poly1.m_vertexCount;

            fixed (Vector2* vertices1 = poly1.m_vertices)
            {
                int iv1 = edge1;
                int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

                Vector2 v11 = vertices1[iv1];
                Vector2 v12 = vertices1[iv2];

                Vector2 localTangent = v12 - v11;
                localTangent.Normalize();

#if !OPTIMIZE
                Vector2 localNormal = Vector2Extensions.Cross(localTangent, 1.0f);
#else
                Vector2 localNormal;
                localNormal.X = localTangent.Y;
                localNormal.Y = -localTangent.X;
#endif
                Vector2 planePoint = 0.5f * (v11 + v12);

#if !OPTIMIZE
                Vector2 tangent = Math.Mul(xf1.R, localTangent);
                Vector2 normal = Vector2Extensions.Cross(tangent, 1.0f);

                v11 = Math.Mul(xf1, v11);
                v12 = Math.Mul(xf1, v12);

                // Face offset.
                float frontOffset = Vector2.Dot(normal, v11);

                // Side offsets, extended by polytope skin thickness.
                float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
                float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;

#else
                Vector2 tangent = Vector2.Zero;
                tangent.X = xf1.R.col1.X * localTangent.X + xf1.R.col2.X * localTangent.Y;
                tangent.Y = xf1.R.col1.Y * localTangent.X + xf1.R.col2.Y * localTangent.Y;

                Vector2 normal;
                normal.X = tangent.Y;
                normal.Y = -tangent.X;

                Vector2 t1; t1.X = (xf1.R.col1.X * v11.X + xf1.R.col2.X * v11.Y); t1.Y = (xf1.R.col1.Y * v11.X + xf1.R.col2.Y * v11.Y);
                v11.X = xf1.position.X + t1.X;
                v11.Y = xf1.position.Y + t1.Y;

                Vector2 t2; t2.X = (xf1.R.col1.X * v12.X + xf1.R.col2.X * v12.Y); t2.Y = (xf1.R.col1.Y * v12.X + xf1.R.col2.Y * v12.Y);
                v12.X = xf1.position.X + t2.X;
                v12.Y = xf1.position.Y + t2.Y;

                // Face offset.
                float frontOffset = normal.X * v11.X + normal.Y * v11.Y; // Vector2.Dot(ref normal, ref v11, out frontOffset);

                // Side offsets, extended by polytope skin thickness.
                float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius; // -Vector2.Dot(tangent, v11) + totalRadius;
                float sideOffset2 = (tangent.X * v12.X + tangent.Y * v12.Y) + totalRadius;  // Vector2.Dot(tangent, v12) + totalRadius;
#endif

                // Clip incident edge against extruded edge1 side edges.
                //ClipVertex[] clipPoints1 = new ClipVertex[2];
                //ClipVertex[] clipPoints2 = new ClipVertex[2];
                int np;

                // Clip to box side 1
                Vector2 nTangent = -tangent;
                np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, ref nTangent, ref sideOffset1, ref iv1);

                if (np < 2)
                    return;

                // Clip to negative box side 1
                np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, ref tangent, ref sideOffset2, ref iv2);

                if (np < 2)
                {
                    return;
                }

                // Now clipPoints2 contains the clipped points.
#if !OPTIMIZE
                manifold.localNormal = localNormal;
                manifold.localPoint = planePoint;
#else
                manifold.localNormal.X = localNormal.X; manifold.localNormal.Y = localNormal.Y;
                manifold.localPoint.X = planePoint.X; manifold.localPoint.Y = planePoint.Y;
#endif

                int pointCount = 0;
                for (int i = 0; i < PhysicSettings.MaxManifoldPoints; ++i)
                {
#if !OPTIMIZE
                    float separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;
#else
                    float separation = (normal.X * clipPoints2[i].v.X + normal.Y * clipPoints2[i].v.Y) - frontOffset; // Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;
#endif
                    if (separation <= totalRadius)
                    {
                        ManifoldPoint cp = manifold.points[pointCount];
#if !OPTIMIZE
                        cp.localPoint = Math.MulT(xf2, clipPoints2[i].v);
#else
                        Vector2 temp = clipPoints2[i].v - xf2.position;
                        Vector2.Dot(ref temp, ref xf2.R.col1, out cp.localPoint.X);
                        Vector2.Dot(ref temp, ref xf2.R.col2, out cp.localPoint.Y);
#endif
                        cp.id = clipPoints2[i].id;
                        if (flip > 0)
                        {
                            // Swap features
                            ContactFeature cf = cp.id.cf;
                            cp.id.cf.indexA = cf.indexB;
                            cp.id.cf.indexB = cf.indexA;
                            cp.id.cf.typeA = cf.typeB;
                            cp.id.cf.typeB = cf.typeA;
                        }
                        manifold.points[pointCount] = cp.Clone();
                        ++pointCount;
                    }
                }

                manifold.pointCount = pointCount;
            }
        }
#else
        ///<summary>
        /// Find edge normal of max separation on A - return if separating axis is found<br/>
        /// Find edge normal of max separation on B - return if separation axis is found<br/>
        /// Choose reference edge as min(minA, minB)<br/>
        /// Find incident edge<br/>
        /// Clip<br/>
        /// <br/>
        /// The normal points from 1 to 2<br/>
        /// <param name="manifold"></param>
        /// <param name="polyA"></param>
        /// <param name="xfA"></param>
        /// <param name="polyB"></param>
        /// <param name="xfB"></param>
        /// </summary>
        public static void CollidePolygons(out Manifold manifold,
					                       PolygonShape polyA, ref Transform xfA,
					                       PolygonShape polyB, ref Transform xfB)
        {
            manifold = new Manifold();

	        manifold.pointCount = 0;
	        float totalRadius = polyA.m_radius + polyB.m_radius;

	        int edgeA = 0;
	        float separationA = FindMaxSeparation(out edgeA, ref polyA, ref xfA, ref polyB, ref xfB);
	        if (separationA > totalRadius)
		        return;

	        int edgeB = 0;
	        float separationB = FindMaxSeparation(out edgeB, ref polyB, ref xfB, ref polyA, ref xfA);
	        if (separationB > totalRadius)
		        return;

	        PolygonShape poly1;	// reference polygon
	        PolygonShape poly2;	// incident polygon
	        Transform xf1, xf2;
	        int edge1;		// reference edge
	        byte flip;
	        const float k_relativeTol = 0.98f;
	        const float k_absoluteTol = 0.001f;

	        if (separationB > k_relativeTol * separationA + k_absoluteTol)
	        {
		        poly1 = polyB;
		        poly2 = polyA;
		        xf1 = xfB;
		        xf2 = xfA;
		        edge1 = edgeB;
		        manifold.type = ManifoldType.e_faceB;
		        flip = 1;
	        }
	        else
	        {
		        poly1 = polyA;
		        poly2 = polyB;
		        xf1 = xfA;
		        xf2 = xfB;
		        edge1 = edgeA;
		        manifold.type = ManifoldType.e_faceA;
		        flip = 0;
	        }

	        ClipVertex[] incidentEdge = new ClipVertex[2];
	        FindIncidentEdge(out incidentEdge, ref poly1, ref xf1, edge1, ref poly2, ref xf2);

	        int count1 = poly1.m_vertexCount;

            Vector2[] vertices1 = poly1.m_vertices;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            Vector2 v11 = vertices1[iv1];
            Vector2 v12 = vertices1[iv2];

            Vector2 localTangent = v12 - v11;
            localTangent.Normalize();

#if !OPTIMIZE
            Vector2 localNormal = Vector2Extensions.Cross(localTangent, 1.0f);
#else
            Vector2 localNormal;
            localNormal.X = localTangent.Y;
            localNormal.Y = -localTangent.X;
#endif
            Vector2 planePoint = 0.5f * (v11 + v12);

#if !OPTIMIZE
            Vector2 tangent = Math.Mul(xf1.R, localTangent);
            Vector2 normal = Vector2Extensions.Cross(tangent, 1.0f);

            v11 = Math.Mul(xf1, v11);
            v12 = Math.Mul(xf1, v12);

            // Face offset.
            float frontOffset = Vector2.Dot(normal, v11);

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
            float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;

#else
            Vector2 tangent = Vector2.Zero;
            tangent.X = xf1.R.col1.X * localTangent.X + xf1.R.col2.X * localTangent.Y;
            tangent.Y = xf1.R.col1.Y * localTangent.X + xf1.R.col2.Y * localTangent.Y;

            Vector2 normal;
            normal.X = tangent.Y;
            normal.Y = -tangent.X;

            Vector2 t1; t1.X = (xf1.R.col1.X * v11.X + xf1.R.col2.X * v11.Y); t1.Y = (xf1.R.col1.Y * v11.X + xf1.R.col2.Y * v11.Y);
            v11.X = xf1.position.X + t1.X;
            v11.Y = xf1.position.Y + t1.Y;

            Vector2 t2; t2.X = (xf1.R.col1.X * v12.X + xf1.R.col2.X * v12.Y); t2.Y = (xf1.R.col1.Y * v12.X + xf1.R.col2.Y * v12.Y);
            v12.X = xf1.position.X + t2.X;
            v12.Y = xf1.position.Y + t2.Y;

            // Face offset.
            float frontOffset = normal.X * v11.X + normal.Y * v11.Y; // Vector2.Dot(ref normal, ref v11, out frontOffset);

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius; // -Vector2.Dot(tangent, v11) + totalRadius;
            float sideOffset2 = (tangent.X * v12.X + tangent.Y * v12.Y) + totalRadius;  // Vector2.Dot(tangent, v12) + totalRadius;
#endif

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1 = new ClipVertex[2];
            ClipVertex[] clipPoints2 = new ClipVertex[2];
            int np;

            // Clip to box side 1
            Vector2 nTangent = -tangent;
            np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, ref nTangent, ref sideOffset1, ref iv1);

            if (np < 2)
                return;

            // Clip to negative box side 1
            np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, ref tangent, ref sideOffset2, ref iv2);

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
#if !OPTIMIZE
            manifold.localNormal = localNormal;
            manifold.localPoint = planePoint;
#else
            manifold.localNormal.X = localNormal.X; manifold.localNormal.Y = localNormal.Y;
            manifold.localPoint.X = planePoint.X; manifold.localPoint.Y = planePoint.Y;
#endif

            int pointCount = 0;
            for (int i = 0; i < PhysicSettings.MaxManifoldPoints; ++i)
            {
#if !OPTIMIZE
                float separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;
#else
                float separation = (normal.X * clipPoints2[i].v.X + normal.Y * clipPoints2[i].v.Y) - frontOffset; // Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;
#endif
                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.points[pointCount];
#if !OPTIMIZE
                    cp.localPoint = Math.MulT(xf2, clipPoints2[i].v);
#else
                    Vector2 temp = clipPoints2[i].v - xf2.position;
                    Vector2.Dot(ref temp, ref xf2.R.col1, out cp.localPoint.X);
                    Vector2.Dot(ref temp, ref xf2.R.col2, out cp.localPoint.Y);
#endif
                    cp.id = clipPoints2[i].id;
                    if (flip > 0)
                    {
                        // Swap features
                        ContactFeature cf = cp.id.cf;
                        cp.id.cf.indexA = cf.indexB;
                        cp.id.cf.indexB = cf.indexA;
                        cp.id.cf.typeA = cf.typeB;
                        cp.id.cf.typeB = cf.typeA;
                    }
                    manifold.points[pointCount] = cp.Clone();
                    ++pointCount;
                }
            }

            manifold.pointCount = pointCount;
        }

#endif
    }
}
